;;;; -*- Mode: Lisp; Package: :dreaming-tree -*-
;;;;
;;;; Intersection Methods
;;;;

(in-package :dreaming-tree)

;; The pseudo-code in [EBE07] first shows a signature with max-time for a general case
;; of testing intersection of two convex polygons (-hedrons?), but later shows a signature
;; without a max-time parameter for testing intersection of two bounding boxes.  Maybe this
;; will become more clear as I move towards a more complete collision detection system.

(defparameter *angle-cutoff* 0.999999s0) ;; Cutoff for cosine of angles


;;; Boxes and planes
(defun %test-obb-plane (e u c n d)
  "Test intersection between oriented box centered at <c> with extent <e> and axes <u> and
plane with normal <n> and plane constant <d>."
  (let* ((r (+ (* (mref e 0) (dot-product n (get-column u 0)))
	       (* (mref e 1) (dot-product n (get-column u 1)))
	       (* (mref e 2) (dot-product n (get-column u 2))))) ;; Radius of projection interval along <n>
	 (s (- (dot-product n c) d))) ;; Distance from center to plane.
    (if (<= (abs s) r)
	(values t 0)
	(values nil (if (<= r s) 1 -1)))))

(defun %test-aabb-plane (c e n d)
  "Test intersection between axis-aligned box centered at <c> with extent <e> and
plane with normal <n> and plane constant <d>."
  (let ((r (+ (* (mref e 0) (abs (mref n 0)))
	      (* (mref e 1) (abs (mref n 1)))
	      (* (mref e 2) (abs (mref n 2)))))
	(s (- (dot-product n c) d)))
    (if (<= (abs s) r)
	(values t 0)
	(values nil (if (<= r s) 1 -1)))))


;;; Axis-aligned bounding boxes
(defun %test-aabb-aabb (a b)
  (let ((cdiff (m- (aabb-c a) (aabb-c b)))
	(rsum (m+ (aabb-e a) (aabb-e b))))
    (not (or (> (abs (x cdiff)) (x rsum))
	     (> (abs (y cdiff)) (y rsum))
	     (> (abs (z cdiff)) (z rsum))))))


;;; Something not right here...
(defun %test-intersection-obb (box0.c box0.u box0.e box1.c box1.U box1.e)
  "Test for intersection between two oriented bounding boxes, each box
defined by a center, set of orienting axes and extents.  Function
parameter names have been chosen to be consistent with nomenclature in
[Ebe07]."
  (let ((diff (m- box1.c box0.c))
	(c (make-matrix 3 3))
	(abs-c (make-matrix 3 3))
	(d (make-vector 0.0 0.0 0.0))
	(parallel-pair-p nil))
    (labels ((r1-dispatcher (n0 n1)
	       (cond ((= n1 0) (+ (* (mref box1.e 1) (mref abs-c n0 2))
				  (* (mref box1.e 2) (mref abs-c n0 1))))
		     ((= n1 1) (+ (* (mref box1.e 0) (mref abs-c n0 2))
				  (* (mref box1.e 2) (mref abs-c n0 0))))
		     ((= n1 2) (+ (* (mref box1.e 0) (mref abs-c n0 1))
				  (* (mref box1.e 1) (mref abs-c n0 0))))))
	     (test-box0-axis (n)
	       (let ((box-0-axis (get-column box0.U n)))
		 (dotimes (i 3)
		   (setf (mref c n i) (dot-product box-0-axis (get-column box1.U i))
			 (mref abs-c n i) (abs (mref c n i)))
		   (if (> (mref abs-c n i) *angle-cutoff*)
		       (setq parallel-pair-p t)))
		 (setf (mref d n) (dot-product diff box-0-axis))
		 (let ((r (abs (mref d n)))
		       (r0 (mref box0.e n))
		       (r1 (+ (* (mref box1.e 0) (mref abs-c n 0))
			      (* (mref box1.e 1) (mref abs-c n 1))
			      (* (mref box1.e 2) (mref abs-c n 2)))))
		   ;;  (format t "Parallel-pairs-p=~A (absC=~A)~%" parallel-pair-p abs-c)
		   ;;  (format t "Test box0 axis ~A=~A~%" n (> r (+ r0 r1)))
		   (not (> r (+ r0 r1))))))
	     (test-box1-axis (n)
	       (let* ((box1-axis (get-column box1.U n))
		      (r (dot-product diff box1-axis ))
		      (r0 (+ (* (mref box0.e 0) (mref abs-c 0 n))
			     (* (mref box0.e 1) (mref abs-c 1 n))
			     (* (mref box0.e 1) (mref abs-c 2 n))))
		      (r1 (mref box1.e n)))
		 ;; (format t "Test box1 axis ~A=~A~%" n (> r (+ r0 r1)))
		 (not (> r (+ r0 r1)))))
	     (test-edge-edge-pair (n0 n1)
	       (let ((r1 (r1-dispatcher n0 n1))
		     r r0)
		 (cond ((= n0 0)
			(setq r (abs (- (* (mref d 2) (mref c 1 n1))
					(* (mref d 1) (mref c 2 n1))))
			      r0 (+ (* (mref box0.e 1) (mref abs-c 2 n1))
				    (* (mref box0.e 2) (mref abs-c 1 n1)))))
		       ((= n0 1)
			(setq r (abs (- (* (mref d 0) (mref c 2 n1))
					(* (mref d 2) (mref c 0 n1))))
			      r0 (+ (* (mref box0.e 0) (mref abs-c 2 n1))
				    (* (mref box0.e 2) (mref abs-c 0 n1)))))
		       ((= n0 2)
			(setq r (abs (- (* (mref d 1) (mref c 0 n1))
					(* (mref d 0) (mref c 1 n1))))
			      r0 (+ (* (mref box0.e 0) (mref abs-c 1 n1))
				    (* (mref box0.e 1) (mref abs-c 0 n1))))))
		 ;;  (format t "Test edge-edge (~A ~A)=~A (r=~A, r0=~A, r1=~A)~%" n0 n1 (> r (+ r0 r1)) r r0 r1)
		 (not (> r (+ r0 r1))))))
      (and (test-box0-axis 0) (test-box0-axis 1) (test-box0-axis 2)
	   (test-box1-axis 0) (test-box1-axis 1) (test-box1-axis 2)
	   (test-edge-edge-pair 0 0) (test-edge-edge-pair 0 1) (test-edge-edge-pair 0 2)
	   (test-edge-edge-pair 1 0) (test-edge-edge-pair 1 1) (test-edge-edge-pair 1 2)
	   (test-edge-edge-pair 2 0) (test-edge-edge-pair 2 1) (test-edge-edge-pair 2 2)))))

;;; Ray intersection tests

(defun %intersect-ray-sphere (ray sphere)
  (let* ((m (m- (point ray) (center sphere)))
	 (b (dot m  (direction ray)))
	 (c (- (dot m m)
	       (* (radius sphere) (radius sphere)))))
    (unless (and (> c 0.0) (b 0.0))
      (let ((discr (- (* b b) c))
	    td
	    q)
	(unless (< discr 0.0)
	  (setq td (- (- b) (sqrt discr)))
	  (if (< td 0.0) (setq td 0.0))
	  (values td (m+ (point ray) (* td (direction ray)))))))))

(defun %find-intersect-ray-aabb (p d a)
  "Test if ray originating from point <p> with direction <d> intersects AABB <a>. When there is an intersection return distance and point of intersection."
  (let ((tmin 0.0) (tmax most-positive-single-float)
	(point (zero-vector 3 :element-type 'single-float))
	(min (aabb-min a))
	(max (aabb-max a)))
    (dotimes (i 3 (values tmin (m+ p (m* d tmin))))
      (let ((di (mref d i))
	    (p_i (mref p i))
	    (min_i (mref min i))
	    (max_i (mref max i)))
	(if (< (abs di) +epsilon+)
	    (if (or (< p_i min_i)
		    (> p_i max_i))
		(return (values nil nil)))
	    (let* ((ood (/ 1.0 di))
		   (t1 (* (- min_i p_i) ood))
		   (t2 (* (- max_i p_i) ood)))
	      (if (> t1 t2)
		  (psetf t2 t1
			 t1 t2)) ; Swap!
	      (setf tmin (max tmin t1)
		    tmax (min tmax t2))
	      (if (> tmin tmax)
		  (return (values nil nil)))))))))
;;; Note: 
(defmethod test-intersection ((b0 oobb) (b1 oobb) &optional max-time)
  (declare (ignore max-time))
  (let ((box0.c (oobb-c b0))
	(box0.u (oobb-u b0))
	(box0.e (oobb-e b0))
	(box1.c (oobb-c b1))
	(box1.u (oobb-u b1))
	(box1.e (oobb-e b1)))
    (if (<= (magnitude (m- box0.c box1.c))
	    (max (radius b0) (radius b1)))
	(%test-intersection-obb box0.c box0.u box0.e box1.c box1.u box1.e))))

(defmethod test-intersection ((s0 spatial) (s1 spatial) &optional max-time)
  (declare (ignore max-time))
  (test-intersection (bounding-volume s0) (bounding-volume s1)))

;;; Finding intersection points

(defmethod find-intersection ((r ray) (box aabb) &optional max-time)
  (declare (ignore max-time))
  (%find-intersect-ray-aabb (point r) (direction r) box))

;;;; ---*--- Intersection using Intervals ---*---

(defconstant +begin-endpoint+ 0)
(defconstant +end-endpoint+ 1)

(defstruct endpoint
  type ; Integer [0, 1].
  value
  object) ; Object that is contained by the endpoint.


(defun e< (e1 e2)
  (or (< (endpoint-value e1) (endpoint-value e2))
      (and (< (- (endpoint-value e1) (endpoint-value e2)) +epsilon+)
	   (< (endpoint-type e1) (endpoint-type e2)))))

;; Intervals are defined as (list <vector?> (make-endpoint ...) (make-endpoint ...))

(defmethod intervals-for ((s spatial))
  (destructuring-bind (xs xe ys ye zs ze) (get-intervals (bounding-volume s))
    (setf (intervals s) (list (list (make-endpoint :type +begin-endpoint+
						   :value xs
						   :object s)
				    (make-endpoint :type +end-endpoint+
						   :value xe
						   :object s))
			      (list (make-endpoint :type +begin-endpoint+
						   :value ys
						   :object s)
				    (make-endpoint :type +end-endpoint+
						   :value ye
						   :object s))
			      (list  (make-endpoint :type +begin-endpoint+
						    :value zs
						    :object s)
				     (make-endpoint :type +end-endpoint+
						    :value ze
						    :object s))))))

(defmethod intervals :around ((s spatial))
  (if (or (slot-boundp s 'intervals)
	  (not (null (slot-value s 'intervals))))
    (intervals-for s)
    (call-next-method)))

(defmethod get-intervals ((obb oobb) &key 
			  (directions (list (unit-x-vector) (unit-y-vector) (unit-z-vector))))
  "Return the intervals (extremum) for Oriented Bounding Box <obb>
along the three directions optionally specified as a list in the
<directions> argument.  If <directions> is not specified then it is
assumed to be the x, y, and z axes."
  (let ((c (oobb-c obb))
	(e (oobb-e obb)))
    (let ((xproj (dot-product (m* (get-axis obb 0) (x e))
			      (first directions)))
	  (yproj (dot-product (m* (get-axis obb 1) (y e))
			      (second directions)))
	  (zproj (dot-product (m* (get-axis obb 2) (z e))
			      (third directions))))
      (list (- (x c) xproj) (+ (x c) xproj)
	    (- (y c) yproj) (+ (y c) yproj)
	    (- (z c) zproj) (+ (z c) zproj)))))

(defparameter *collect-stats* t)

(defun sort-and-sweep (object-set)
  "Given set of objects, test for intersections by doing a sort and
sweep of their interval endpoints."
  (let* ((intervals (mapcar #'get-intervals object-set))
	 (x-intervals (sort (alexandria:flatten (mapcar #'interval3d-x intervals)) #'e<))
	 active-x-intervals axi-length
	 intersecting-objects)
    (dolist (xi x-intervals (values intersecting-objects
				    (length x-intervals)
				    axi-length))
      (if (eq (endpoint-type xi) +begin-endpoint+)
	  (push (endpoint-interval xi) active-x-intervals)
	  (setf active-x-intervals (delete (endpoint-interval xi) active-x-intervals)))
					;(format t "X :: ~A~%" active-x-intervals)
      (if *collect-stats*
	  (push (length active-x-intervals) axi-length))
      (when (> (length active-x-intervals) 1)
	(dolist (Ii (rest active-x-intervals))
	  (if (interval-intersect-p (endpoint-interval xi) Ii)
	      (pushnew (list (interval3d-object (endpoint-interval xi)) (interval3d-object Ii)) intersecting-objects
		       :test #'(lambda (x y)
				 (or (and (eql (first x) (first y))
					  (eql (second x) (second y)))
				     (and (eql (first x) (second y))
					  (eql (second x) (first y))))))))))))

(defun sweep-intervals-for-intersection (xintervals yintervals zintervals)
  ""
  (labels ((equal-intersection (x y)
	     (or (and (eql (car x) (car y))
		      (eql (cdr x) (cdr y)))
		 (and (eql (car x) (cdr y))
		      (eql (cdr x) (car y)))))
	   (do-sweep (endpoints)
	     (let ((active-intervals)
		   (intersecting-objects))
	       (dolist (i endpoints intersecting-objects)
		 (if (eq (endpoint-type i) +begin-endpoint+)
		     (push (endpoint-object i) active-intervals)
		     (setf active-intervals (delete (endpoint-object i) active-intervals)))
					;(format t "X :: ~A~%" active-x-intervals)
		 (when (> (length active-intervals) 1)
		   (let ((obj0 (first active-intervals)))
		     (dolist (Ii (rest active-intervals))
		       (push (cons obj0 Ii)
			     intersecting-objects))))))))
    (nintersection (nintersection (do-sweep xintervals) (do-sweep yintervals)
				  :test #'equal-intersection)
		   (do-sweep zintervals)
		   :test #'equal-intersection)))

;; Definitely not a complete list of cases, but it sure
;; helps with troubleshooting.  Should come up with cases for point-edge
;; and edge-edge intersection.
(defun test-obb-test-intersect ()
  (let ((b0 (make-oobb ;:name "Box0"
	     :c (zero-vector 3)
	     :u (identity-matrix 3)
	     :e (make-vector 1.0 1.0 1.0)))
	(b1 (make-oobb ;:name "Box1"
	     :c (make-vector 5.0 5.0 5.0)
	     :u (identity-matrix 3)
	     :e (make-vector 1.0 1.0 1.0)))
	(b2 (make-oobb ;:name "Box2"
	     :c (zero-vector 3)
	     :u (identity-matrix 3)
	     :e (make-vector 5.1 5.1 5.1)))
	(b3 (make-oobb ;:name "Box3"
	     :c (zero-vector 3)
	     :u (euler-rotate-matrix 45.0 0.0 0.0)
	     :e (make-vector 1.0 1.0 1.0))))
    (format t "Extreme points of b0=~A~%" (get-intervals b0))
    (format t "Extreme points of b1=~A~%" (get-intervals b1))
    (format t "Extreme points of b2=~A~%" (get-intervals b2))
    (format t "Extreme points of b3=~A~%" (get-intervals b3))
    (format t "b0 and b1 = ~A~%" (test-intersection b0 b1))
    (format t "b0 and b0 = ~A~%" (test-intersection b0 b0))
    (format t "b1 and b2 = ~A~%" (test-intersection b1 b2))
    (format t "b1 and b3 = ~A~%" (test-intersection b1 b3))
    (sort-and-sweep (list b0 b1 b2 b3))))

(defun test-intersect ()
  (let ((c0 (make-cube 0.0 0.0 0.0))
	(c1 (make-cube 0.5 0.0 0.0))
	(c2 (make-cube 1.0 0.0 0.0))
	(c3 (make-cube 1.1 0.0 0.0))
	(c4 (make-cube 5.0 0.0 0.0)))
    (assert (test-intersection c0 c1))
    (assert (test-intersection c0 c2))
    (assert (not (test-intersection c0 c3)))
    (assert (not (test-intersection c0 c4)))))

(defun random-obb (max-c max-e)
  (let ((c (make-vector (random max-c) (random max-c) (random max-c)))
	(e (make-vector (random max-e) (random max-e) (random max-e)))
	(u (euler-rotate-matrix (random (* 2.0 pi))
				(random (* 2.0 pi))
				(random (* 2.0 pi))
				t)))
    (make-oobb :c c :e e :u u)))

(defun make-box-set (n max-center max-extent)
  (iter (for i from 0 to n)
	(collecting (random-obb max-center max-extent))))

(defun ordered-box-set (n spacing max-extent)
  (alexandria:flatten
   (iter (for i from 0 below n)
	 (collecting 
	  (alexandria:flatten 
	   (iter (for j from 0 below n)
		 (collecting 
		  (iter (for k from 0 below n)
			(collecting 
			 (make-oobb :c (make-vector (* i spacing)
						    (* j spacing)
						    (* k spacing))
				    :e (make-vector (random max-extent) 
						    (random max-extent) 
						    (random max-extent))
				    :u (euler-rotate-matrix (random (* 2.0 pi))
							    (random (* 2.0 pi))
							    (random (* 2.0 pi))
							    t)))))))))))

(defun line-of-boxes (n step max-extent)
  (let (boxes)
    (dotimes (i n boxes)
      (push (make-oobb :c (make-vector (* i step) 0.0 0.0)
		       :e (make-vector (random max-extent) 
					    (random max-extent) 
					    (random max-extent))
		       :u (identity-matrix 3))
	    boxes))))

(defparameter *small-box-set* (make-box-set 10 50.0 5.0))
(defparameter *medium-box-set* (make-box-set 100 50.0 5.0))
(defparameter *large-box-set* (make-box-set 1000 50.0 5.0))
(defparameter *huge-box-set* (make-box-set 10000 50.0 5.0))
(defparameter *sparse-large-box-set* (make-box-set 1000 100.0 1.0))

(defun stress-test ()
  (time (dotimes (i 10) (sort-and-sweep *small-box-set*)))
  (time (dotimes (i 10) (sort-and-sweep *medium-box-set*)))
  (time (dotimes (i 10) (sort-and-sweep *large-box-set*)))
  (time (dotimes (i 10) (sort-and-sweep *huge-box-set*)))
  (time (dotimes (i 10) (sort-and-sweep *sparse-large-box-set*)))
  nil)

(defun st1 ()
  (multiple-value-bind (objs n an) (sort-and-sweep *sparse-large-box-set*)
    (let ((sum 0.0))
      (format t "Found ~A intersections.~%" (length objs))
      (format t "Sorted ~A intervals, with average active length of ~A.~%" n
	      (dolist (a an (/ sum (length an)))
		(incf sum a))))))

(defun st2 ()
  (multiple-value-bind (objs n an) (sort-and-sweep (line-of-boxes 1000 10.0 1.0))
    (let ((sum 0.0))
      (format t "Found ~A intersections.~%" (length objs))
      (format t "Sorted ~A intervals, with average active length of ~A.~%" n
	      (dolist (a an (/ sum (length an)))
		(incf sum a))))))
